import numpy as np
# import matplotlib.pyplot as plt

class Manipulator:
    def __init__(self, l1 = 2.0, l2 = 1.0):
        self.__L1 = l1
        self.__L2 = l2

        self.t1 = 0
        self.t2 = 0

    def get_link1_pos(self, theta1, theta2):
        x_l1 = self.__L1 * np.math.cos(theta1)
        y_l1 = self.__L1 * np.math.sin(theta1)

        return np.array([x_l1, y_l1])

    def get_ee_pos(self, theta1, theta2):
        x_ee = self.__L1 * np.math.cos(theta1) + self.__L2 * np.math.cos(theta1+theta2)
        y_ee = self.__L1 * np.math.sin(theta1) + self.__L2 * np.math.sin(theta1+theta2)

        return np.array([x_ee, y_ee])